import sys
import numpy as np
import cv2
from PySide6.QtWidgets import QApplication, QMainWindow, QPushButton, QVBoxLayout, QWidget, QSlider, QLabel, QTextEdit, QHBoxLayout, QGroupBox, QGridLayout, QListWidget, QCheckBox, QDialog, QTableWidget, QTableWidgetItem, QHeaderView, QFileDialog
from PySide6.QtCore import Qt, QTimer
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
from matplotlib.figure import Figure
import matplotlib.pyplot as plt
import os
import pandas as pd

# 靶标坐标系下的6个点坐标（单位：毫米）
target_points = np.array([
    [0,   0,   0],    # P1
    [500, 0,   0],    # P2
    [500, 500, 0],    # P3
    [0,   500, 0],    # P4
    [-250,   250,   -300],  # P5
    [750, 250, -300],  # P6
])

# 相机参数
IMAGE_WIDTH = 1920  # 像素宽度
IMAGE_HEIGHT = 1080  # 像素高度
PIXEL_SIZE = 2.9e-3  # 像元尺寸（毫米）
FOCAL_LENGTH_MM = 35  # 焦距（毫米）

# 计算相机内参矩阵
FOCAL_LENGTH_PIXELS_X = FOCAL_LENGTH_MM / PIXEL_SIZE  # X方向焦距（像素）
FOCAL_LENGTH_PIXELS_Y = FOCAL_LENGTH_MM / PIXEL_SIZE  # Y方向焦距（像素）
PRINCIPAL_POINT_X = IMAGE_WIDTH / 2  # 主点X坐标（像素）
PRINCIPAL_POINT_Y = IMAGE_HEIGHT / 2  # 主点Y坐标（像素）

INTRINSIC_MATRIX = np.array([
    [FOCAL_LENGTH_PIXELS_X, 0, PRINCIPAL_POINT_X],
    [0, FOCAL_LENGTH_PIXELS_Y, PRINCIPAL_POINT_Y],
    [0, 0, 1]
])

# 定义靶标姿态
def get_target_pose(pose_id):
    if pose_id == 0:
        # 初始姿态：靶标平面平行于图像平面，突出点正对相机
        # 将靶标放置在相机前方2000mm处
        return np.array([
            [1, 0, 0, 0],
            [0, 1, 0, 0],
            [0, 0, 1, 2000],
            [0, 0, 0, 1]
        ])
    elif pose_id == 1:
        # 姿态1：绕X轴旋转30度
        theta = np.radians(30)
        return np.array([
            [1, 0, 0, 0],
            [0, np.cos(theta), -np.sin(theta), 0],
            [0, np.sin(theta), np.cos(theta), 2000],
            [0, 0, 0, 1]
        ])
    elif pose_id == 2:
        # 姿态2：绕Y轴旋转30度
        theta = np.radians(30)
        return np.array([
            [np.cos(theta), 0, np.sin(theta), 0],
            [0, 1, 0, 0],
            [-np.sin(theta), 0, np.cos(theta), 2000],
            [0, 0, 0, 1]
        ])
    else:
        return np.eye(4)

class DataDialog(QDialog):
    def __init__(self, parent=None):
        super().__init__(parent)
        self.setWindowTitle("靶标数据")
        self.setGeometry(200, 200, 1200, 400)
        print("DataDialog initialized")  # 调试信息
        
        # 设置对话框样式
        self.setStyleSheet("""
            QDialog {
                background-color: white;
            }
            QTableWidget {
                border: 1px solid #cccccc;
                gridline-color: #f0f0f0;
            }
            QHeaderView::section {
                background-color: #f0f0f0;
                padding: 4px;
                border: 1px solid #cccccc;
                font-weight: bold;
            }
            QPushButton {
                padding: 5px;
                margin: 2px;
            }
        """)
        
        # 创建表格
        self.table = QTableWidget()
        self.table.setColumnCount(5)
        self.table.setHorizontalHeaderLabels([
            "靶标点像素坐标", 
            "靶标坐标系相对于相机坐标系的R_tc", 
            "靶标坐标系相对于相机坐标系的T_tc",
            "靶标原点在全站仪坐标系下的坐标Ot_w",
            "靶标三号点在全站仪坐标系下的坐标P3_w"
        ])
        
        # 设置表格列宽
        header = self.table.horizontalHeader()
        header.setSectionResizeMode(0, QHeaderView.Stretch)
        header.setSectionResizeMode(1, QHeaderView.Stretch)
        header.setSectionResizeMode(2, QHeaderView.Stretch)
        header.setSectionResizeMode(3, QHeaderView.Stretch)
        header.setSectionResizeMode(4, QHeaderView.Stretch)
        
        # 设置表格选择模式
        self.table.setSelectionBehavior(QTableWidget.SelectRows)  # 整行选择
        self.table.setSelectionMode(QTableWidget.ExtendedSelection)  # 允许多行选择
        
        # 创建按钮布局
        button_layout = QHBoxLayout()
        
        # 创建删除选中行按钮
        self.delete_selected_button = QPushButton("删除选中行")
        self.delete_selected_button.clicked.connect(self.delete_selected_rows)
        
        # 创建删除所有数据按钮
        self.delete_all_button = QPushButton("删除所有数据")
        self.delete_all_button.clicked.connect(self.clear_table)
        
        # 创建导出数据按钮
        self.export_button = QPushButton("导出数据到CSV")
        self.export_button.clicked.connect(self.export_to_csv)
        
        # 添加按钮到布局
        button_layout.addWidget(self.delete_selected_button)
        button_layout.addWidget(self.delete_all_button)
        button_layout.addWidget(self.export_button)
        
        # 创建主布局
        layout = QVBoxLayout()
        layout.addWidget(self.table)
        layout.addLayout(button_layout)
        self.setLayout(layout)
        
        # 添加相机外参属性
        self.world_to_camera_rotation = None
        self.world_to_camera_translation = None
    
    def delete_selected_rows(self):
        """删除选中的行"""
        selected_rows = sorted(set(item.row() for item in self.table.selectedItems()), reverse=True)
        if not selected_rows:
            return
            
        for row in selected_rows:
            self.table.removeRow(row)
        print(f"Deleted {len(selected_rows)} rows")  # 调试信息
    
    def clear_table(self):
        """清空表格中的所有数据"""
        self.table.setRowCount(0)
        print("Table cleared")  # 调试信息
    
    def update_data(self, pixel_coords, R, T, P_w, world_rotation=None, world_translation=None, clear_table=False):
        """更新数据，增加相机外参参数"""
        print(f"Updating dialog with {len(pixel_coords)} points")
        try:
            # 保存相机外参
            if world_rotation is not None:
                self.world_to_camera_rotation = world_rotation
            if world_translation is not None:
                self.world_to_camera_translation = world_translation
                
            # 如果需要清空表格
            if clear_table:
                self.clear_table()
            
            # 获取当前行数
            current_row = self.table.rowCount()
            
            # 添加一行新数据
            self.table.insertRow(current_row)
            
            # 像素坐标
            pixel_coords_str = "\n".join([f"P{i+1}: ({pixel_coords[i][0]:.2f}, {pixel_coords[i][1]:.2f})" 
                                        for i in range(len(pixel_coords))])
            pixel_item = QTableWidgetItem(pixel_coords_str)
            self.table.setItem(current_row, 0, pixel_item)
            
            # 旋转矩阵R - 修改为保留6位小数
            R_str = "\n".join([f"[{', '.join([f'{x:12.6f}' for x in row])}]" for row in R])
            R_item = QTableWidgetItem(R_str)
            self.table.setItem(current_row, 1, R_item)
            
            # 平移向量T
            T_str = f"[{T[0]:.2f}, {T[1]:.2f}, {T[2]:.2f}]"
            T_item = QTableWidgetItem(T_str)
            self.table.setItem(current_row, 2, T_item)
            
            # 靶标原点坐标
            Pw_str = f"[{P_w[0]:.2f}, {P_w[1]:.2f}, {P_w[2]:.2f}]"
            Pw_item = QTableWidgetItem(Pw_str)
            self.table.setItem(current_row, 3, Pw_item)
            
            # 计算靶标三号点在全站仪坐标系下的坐标
            P3_target = np.array([500, 500, 0])  # 三号点在靶标坐标系中的坐标
            P3_world = P_w + R.T @ P3_target  # 转换到全站仪坐标系
            P3_str = f"[{P3_world[0]:.2f}, {P3_world[1]:.2f}, {P3_world[2]:.2f}]"
            P3_item = QTableWidgetItem(P3_str)
            self.table.setItem(current_row, 4, P3_item)
            
            # 调整行高以适应内容
            self.table.resizeRowToContents(current_row)
            
            print("Dialog data updated successfully")
            
        except Exception as e:
            print(f"Error in update_data: {str(e)}")

    def export_to_csv(self):
        """导出数据到CSV文件"""
        try:
            # 获取当前时间作为文件名
            from datetime import datetime
            current_time = datetime.now().strftime("%Y%m%d_%H%M%S")
            file_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), f"靶标数据_{current_time}.csv")
            
            # 1. 保存靶标数据
            target_data = []
            for row in range(self.table.rowCount()):
                row_data = []
                for col in range(self.table.columnCount()):
                    item = self.table.item(row, col)
                    if item is not None:
                        cell_text = item.text().replace('\n', ' ')
                        if ',' in cell_text:
                            cell_text = f'"{cell_text}"'
                        row_data.append(cell_text)
                    else:
                        row_data.append('')
                target_data.append(row_data)
            
            # 2. 保存相机参数数据
            camera_params = {
                "参数名称": [
                    "图像宽度(像素)",
                    "图像高度(像素)",
                    "像元尺寸(mm)",
                    "焦距(mm)",
                    "X方向焦距(像素)",
                    "Y方向焦距(像素)",
                    "主点X坐标(像素)",
                    "主点Y坐标(像素)"
                ],
                "参数值": [
                    IMAGE_WIDTH,
                    IMAGE_HEIGHT,
                    PIXEL_SIZE,
                    FOCAL_LENGTH_MM,
                    FOCAL_LENGTH_PIXELS_X,
                    FOCAL_LENGTH_PIXELS_Y,
                    PRINCIPAL_POINT_X,
                    PRINCIPAL_POINT_Y
                ]
            }
            
            # 3. 保存相机内参矩阵
            intrinsic_matrix = [
                f"[{INTRINSIC_MATRIX[0,0]:.2f}, {INTRINSIC_MATRIX[0,1]:.2f}, {INTRINSIC_MATRIX[0,2]:.2f}]",
                f"[{INTRINSIC_MATRIX[1,0]:.2f}, {INTRINSIC_MATRIX[1,1]:.2f}, {INTRINSIC_MATRIX[1,2]:.2f}]",
                f"[{INTRINSIC_MATRIX[2,0]:.2f}, {INTRINSIC_MATRIX[2,1]:.2f}, {INTRINSIC_MATRIX[2,2]:.2f}]"
            ]
            
            # 4. 保存相机外参（相对于全站仪）
            # 计算欧拉角
            world_rotation_euler = np.degrees(np.array([
                np.arctan2(self.world_to_camera_rotation[2, 1], self.world_to_camera_rotation[2, 2]),
                np.arctan2(-self.world_to_camera_rotation[2, 0], np.sqrt(self.world_to_camera_rotation[2, 1]**2 + self.world_to_camera_rotation[2, 2]**2)),
                np.arctan2(self.world_to_camera_rotation[1, 0], self.world_to_camera_rotation[0, 0])
            ]))
            
            camera_extrinsic = {
                "参数名称": [
                    "相机位置X(mm)",
                    "相机位置Y(mm)",
                    "相机位置Z(mm)",
                    "相机欧拉角X(度)",
                    "相机欧拉角Y(度)",
                    "相机欧拉角Z(度)"
                ],
                "参数值": [
                    self.world_to_camera_translation[0],
                    self.world_to_camera_translation[1],
                    self.world_to_camera_translation[2],
                    world_rotation_euler[0],
                    world_rotation_euler[1],
                    world_rotation_euler[2]
                ]
            }
            
            # 5. 保存相机坐标系相对于世界坐标系的变换
            camera_to_world = {
                "参数名称": [
                    "相机坐标系原点在世界坐标系中的位置",
                    "相机坐标系X轴在世界坐标系中的方向向量",
                    "相机坐标系Y轴在世界坐标系中的方向向量",
                    "相机坐标系Z轴在世界坐标系中的方向向量"
                ],
                "参数值": [
                    f"[{self.world_to_camera_translation[0]:.8f}, {self.world_to_camera_translation[1]:.8f}, {self.world_to_camera_translation[2]:.8f}]",
                    f"[{self.world_to_camera_rotation[0,0]:.8f}, {self.world_to_camera_rotation[0,1]:.8f}, {self.world_to_camera_rotation[0,2]:.8f}]",
                    f"[{self.world_to_camera_rotation[1,0]:.8f}, {self.world_to_camera_rotation[1,1]:.8f}, {self.world_to_camera_rotation[1,2]:.8f}]",
                    f"[{self.world_to_camera_rotation[2,0]:.8f}, {self.world_to_camera_rotation[2,1]:.8f}, {self.world_to_camera_rotation[2,2]:.8f}]"
                ]
            }
            
            # 写入CSV文件
            with open(file_path, 'w', encoding='utf-8') as f:
                # 写入靶标数据
                f.write("=== 靶标数据 ===\n")
                headers = [
                    "靶标点像素坐标",
                    "靶标坐标系相对于相机坐标系的R_tc",
                    "靶标坐标系相对于相机坐标系的T_tc",
                    "靶标原点在全站仪坐标系下的坐标Ot_w",
                    "靶标三号点在全站仪坐标系下的坐标P3_w"
                ]
                f.write(','.join(headers) + '\n')
                for row in target_data:
                    f.write(','.join(row) + '\n')
                
                # 在写入靶标数据之后，写入全站仪测量数据
                f.write('\n=== 全站仪测量数据 ===\n')
                
                # 遍历每个靶标位置
                for row in range(self.table.rowCount()):
                    # 获取靶标原点在全站仪坐标系下的坐标
                    Otw_item = self.table.item(row, 3)
                    if Otw_item is not None:
                        Otw_str = Otw_item.text().strip('[]')
                        Otw = np.array([float(x) for x in Otw_str.split(',')])
                        
                        # 写入靶标原点坐标
                        f.write(f"靶标位置{row+1}的原点坐标,{Otw[0]:.3f},{Otw[1]:.3f},{Otw[2]:.3f}\n")
                        
                        # 获取靶标坐标系相对于相机坐标系的变换
                        R_tc_item = self.table.item(row, 1)
                        T_tc_item = self.table.item(row, 2)
                        if R_tc_item is not None and T_tc_item is not None:
                            # 解析旋转矩阵
                            R_tc_str = R_tc_item.text().split('\n')
                            R_tc = np.array([
                                [float(x) for x in row.strip('[]').split(',')]
                                for row in R_tc_str
                            ])
                            
                            # 解析平移向量
                            T_tc = np.array([float(x) for x in T_tc_item.text().strip('[]').split(',')])
                            
                            # 计算靶标三号点在靶标坐标系中的坐标（500, 500, 0）
                            P3_target = np.array([500, 500, 0])
                            
                            # 将靶标三号点转换到全站仪坐标系
                            P3_world = Otw + R_tc.T @ P3_target
                            
                            # 写入靶标三号点坐标
                            f.write(f"靶标位置{row+1}的三号点坐标,{P3_world[0]:.3f},{P3_world[1]:.3f},{P3_world[2]:.3f}\n")
                
                # 写入分隔符
                f.write('\n=== 相机参数 ===\n')
                
                # 写入相机参数
                for name, value in zip(camera_params["参数名称"], camera_params["参数值"]):
                    f.write(f"{name},{value}\n")
                
                # 写入内参矩阵
                f.write('\n=== 相机内参矩阵 ===\n')
                for row in intrinsic_matrix:
                    f.write(row + '\n')
                
                # 写入相机外参
                f.write('\n=== 相机外参（相对于全站仪） ===\n')
                # 写入位置和欧拉角
                for name, value in zip(camera_extrinsic["参数名称"], camera_extrinsic["参数值"]):
                    f.write(f"{name},{value:.2f}\n")
                
                # 写入旋转矩阵
                f.write('\n相机旋转矩阵:\n')
                for row in self.world_to_camera_rotation:
                    f.write(f"[{row[0]:.6f}, {row[1]:.6f}, {row[2]:.6f}]\n")
                
                # 写入相机坐标系相对于世界坐标系的变换
                f.write('\n=== 相机坐标系相对于世界坐标系的变换 ===\n')
                for name, value in zip(camera_to_world["参数名称"], camera_to_world["参数值"]):
                    f.write(f"{name},{value}\n")
            
            print(f"数据已成功导出到: {file_path}")
            
        except Exception as e:
            print(f"导出数据时出错: {str(e)}")

class MainWindow(QMainWindow):
    def __init__(self):
        super().__init__()
        self.setWindowTitle("Target Projection Demo")
        self.setGeometry(100, 100, 1200, 800)
        self.setStyleSheet("""
            QMainWindow {
                background-color: #f0f0f0;
            }
            QLabel {
                color: #333333;
                font-size: 12px;
                font-weight: bold;
                padding: 0px;
                margin: 0px;
            }
            QSlider {
                height: 20px;
                margin: 0px;
            }
            QSlider::groove:horizontal {
                border: 1px solid #999999;
                height: 8px;
                background: #ffffff;
                margin: 2px 0;
                border-radius: 4px;
            }
            QSlider::handle:horizontal {
                background: #4a90e2;
                border: 1px solid #5c5c5c;
                width: 18px;
                margin: -2px 0;
                border-radius: 9px;
            }
            QTextEdit {
                border: 1px solid #cccccc;
                border-radius: 4px;
                padding: 2px;
                background: #ffffff;
                margin: 0px;
            }
            QPushButton {
                background-color: #4a90e2;
                color: white;
                border: none;
                padding: 5px 10px;
                border-radius: 4px;
                margin: 0px;
            }
            QPushButton:hover {
                background-color: #357abd;
            }
            QGroupBox {
                border: 1px solid #cccccc;
                border-radius: 4px;
                margin-top: 5px;
                font-weight: bold;
                padding-top: 10px;
            }
            QGroupBox::title {
                subcontrol-origin: margin;
                left: 10px;
                padding: 0 3px;
            }
            QVBoxLayout, QHBoxLayout {
                spacing: 2px;
                margin: 0px;
            }
        """)

        # ================== 先初始化外参变量 ==================
        # 相机坐标系相对于世界坐标系的外参数
        self.world_to_camera_translation = np.array([2000.0, 2000.0, 0.0])  # X=2000, Y=2000, Z=0
        self.world_euler = {'X': 45.0, 'Y': 0.0, 'Z': 0.0}  # X=45°, Y=0°, Z=0°
        # 生成初始旋转矩阵
        rx, ry, rz = np.radians(self.world_euler['X']), np.radians(self.world_euler['Y']), np.radians(self.world_euler['Z'])
        Rx = np.array([
            [1, 0, 0],
            [0, np.cos(rx), -np.sin(rx)],
            [0, np.sin(rx), np.cos(rx)]
        ])
        Ry = np.array([
            [np.cos(ry), 0, np.sin(ry)],
            [0, 1, 0],
            [-np.sin(ry), 0, np.cos(ry)]
        ])
        Rz = np.array([
            [np.cos(rz), -np.sin(rz), 0],
            [np.sin(rz), np.cos(rz), 0],
            [0, 0, 1]
        ])
        self.world_to_camera_rotation = Rz @ Ry @ Rx
        # =====================================================

        # 初始化靶标位置和姿态状态
        self.current_position = np.array([0.0, 0.0, 2000.0])  # 初始位置
        self.current_rotation = np.eye(3)  # 初始旋转矩阵
        self.current_pose_id = 0
        
        # 控制当量设置
        self.position_step = 10.0  # 位置控制当量（毫米）
        self.rotation_step = 5.0   # 姿态控制当量（度）

        # 位置范围设置（单位：毫米）
        self.position_ranges = {
            'X': (-4000, 4000),    # X轴范围：-4米到4米
            'Y': (-4000, 4000),    # Y轴范围：-4米到4米
            'Z': (0, 200000)       # Z轴范围：0到200米
        }

        # 姿态范围设置（单位：度）
        self.rotation_range = (-45, 45)  # 所有轴的范围：-45度到45度

        # 初始化世界坐标系到相机坐标系的变换
        self.world_to_camera_rotation = np.eye(3)  # 初始旋转矩阵
        self.world_to_camera_translation = np.zeros(3)  # 初始平移向量

        # 新增：保存世界坐标系欧拉角
        self.world_euler = {'X': 0.0, 'Y': 0.0, 'Z': 0.0}

        # 初始化靶标坐标系欧拉角
        self.current_euler = {'X': 45.0, 'Y': 0.0, 'Z': 0.0}

        # 创建主窗口部件
        central_widget = QWidget()
        self.setCentralWidget(central_widget)
        layout = QVBoxLayout(central_widget)
        layout.setSpacing(2)
        layout.setContentsMargins(5, 5, 5, 5)

        # 创建matplotlib图形
        self.fig = Figure(figsize=(12, 8))
        self.canvas = FigureCanvas(self.fig)
        layout.addWidget(self.canvas)

        # 创建3D和2D子图
        self.ax_3d = self.fig.add_subplot(121, projection='3d')
        self.ax_2d = self.fig.add_subplot(122)

        # 创建相机参数控制组
        camera_group = QGroupBox("相机参数设置")
        camera_layout = QGridLayout()
        camera_layout.setSpacing(2)
        camera_layout.setContentsMargins(5, 5, 5, 5)
        
        # 创建参数控制区域
        params_layout = QHBoxLayout()
        params_layout.setSpacing(10)
        
        # 左侧参数组
        left_params = QVBoxLayout()
        left_params.setSpacing(2)
        
        # 像元尺寸设置
        pixel_size_layout = QHBoxLayout()
        pixel_size_label = QLabel("像元尺寸(mm):")
        self.pixel_size_input = QTextEdit()
        self.pixel_size_input.setMaximumHeight(25)
        self.pixel_size_input.setMaximumWidth(80)
        self.pixel_size_input.setText(f"{PIXEL_SIZE:.4f}")
        self.pixel_size_input.textChanged.connect(lambda: self.update_pixel_size(self.pixel_size_input.toPlainText()))
        
        # 添加像元尺寸滑块
        self.pixel_size_slider = QSlider(Qt.Horizontal)
        self.pixel_size_slider.setMinimum(1)    # 0.001mm
        self.pixel_size_slider.setMaximum(100)  # 0.1mm
        self.pixel_size_slider.setValue(int(PIXEL_SIZE * 1000))
        self.pixel_size_slider.valueChanged.connect(self.on_pixel_size_slider_changed)
        
        pixel_size_layout.addWidget(pixel_size_label)
        pixel_size_layout.addWidget(self.pixel_size_input)
        pixel_size_layout.addWidget(self.pixel_size_slider)
        left_params.addLayout(pixel_size_layout)
        
        # 分辨率设置
        resolution_layout = QHBoxLayout()
        resolution_label = QLabel("分辨率:")
        resolution_input_layout = QHBoxLayout()
        resolution_input_layout.setSpacing(2)
        resolution_input_layout.setContentsMargins(0, 0, 0, 0)
        self.resolution_x_input = QTextEdit()
        self.resolution_y_input = QTextEdit()
        self.resolution_x_input.setMaximumHeight(25)
        self.resolution_y_input.setMaximumHeight(25)
        self.resolution_x_input.setMaximumWidth(70)
        self.resolution_y_input.setMaximumWidth(70)
        self.resolution_x_input.setText(str(IMAGE_WIDTH))
        self.resolution_y_input.setText(str(IMAGE_HEIGHT))
        self.resolution_x_input.textChanged.connect(lambda: self.update_resolution('X'))
        self.resolution_y_input.textChanged.connect(lambda: self.update_resolution('Y'))
        
        # 添加分辨率滑块
        resolution_slider_layout = QHBoxLayout()
        self.resolution_x_slider = QSlider(Qt.Horizontal)
        self.resolution_y_slider = QSlider(Qt.Horizontal)
        self.resolution_x_slider.setMinimum(640)  # 最小640
        self.resolution_x_slider.setMaximum(3840)  # 最大4K
        self.resolution_y_slider.setMinimum(480)  # 最小480
        self.resolution_y_slider.setMaximum(2160)  # 最大4K
        self.resolution_x_slider.setValue(IMAGE_WIDTH)
        self.resolution_y_slider.setValue(IMAGE_HEIGHT)
        self.resolution_x_slider.valueChanged.connect(lambda v: self.update_resolution_slider('X', v))
        self.resolution_y_slider.valueChanged.connect(lambda v: self.update_resolution_slider('Y', v))
        
        resolution_slider_layout.addWidget(self.resolution_x_slider)
        resolution_slider_layout.addWidget(QLabel("×"))
        resolution_slider_layout.addWidget(self.resolution_y_slider)
        
        resolution_input_layout.addWidget(self.resolution_x_input)
        resolution_input_layout.addWidget(QLabel("×"))
        resolution_input_layout.addWidget(self.resolution_y_input)
        resolution_layout.addWidget(resolution_label)
        resolution_layout.addLayout(resolution_input_layout)
        left_params.addLayout(resolution_layout)
        left_params.addLayout(resolution_slider_layout)
        
        # 焦距设置
        focal_length_layout = QHBoxLayout()
        focal_length_label = QLabel("焦距(mm):")
        self.focal_length_input = QTextEdit()
        self.focal_length_input.setMaximumHeight(25)
        self.focal_length_input.setMaximumWidth(80)
        self.focal_length_input.setText(f"{FOCAL_LENGTH_MM:.1f}")
        self.focal_length_input.textChanged.connect(lambda: self.update_focal_length_input(self.focal_length_input.toPlainText()))
        
        # 添加焦距滑块
        self.focal_length_slider = QSlider(Qt.Horizontal)
        self.focal_length_slider.setMinimum(10)  # 最小10mm
        self.focal_length_slider.setMaximum(200)  # 最大200mm
        self.focal_length_slider.setValue(int(FOCAL_LENGTH_MM))
        self.focal_length_slider.valueChanged.connect(lambda v: self.update_focal_length_input(str(v)))
        
        focal_length_layout.addWidget(focal_length_label)
        focal_length_layout.addWidget(self.focal_length_input)
        focal_length_layout.addWidget(self.focal_length_slider)
        left_params.addLayout(focal_length_layout)
        
        params_layout.addLayout(left_params)
        
        # 右侧内参矩阵显示
        right_params = QVBoxLayout()
        intrinsic_label = QLabel("相机内参矩阵:")
        self.intrinsic_text = QTextEdit()
        self.intrinsic_text.setReadOnly(True)
        self.intrinsic_text.setMaximumHeight(70)
        self.intrinsic_text.setStyleSheet("""
            QTextEdit {
                font-family: 'Courier New';
                font-size: 12px;
                background-color: #f8f8f8;
                padding: 2px;
            }
        """)
        right_params.addWidget(intrinsic_label)
        right_params.addWidget(self.intrinsic_text)
        
        params_layout.addLayout(right_params)
        camera_layout.addLayout(params_layout, 0, 0)
        
        camera_group.setLayout(camera_layout)
        layout.addWidget(camera_group)

        # 创建靶标控制组
        target_group = QGroupBox("靶标坐标系设置")
        target_layout = QVBoxLayout()
        target_layout.setSpacing(2)
        target_layout.setContentsMargins(5, 5, 5, 5)

        # 创建位置控制滑块
        position_group = QGroupBox("位置控制")
        position_layout = QHBoxLayout()
        position_layout.setSpacing(2)
        position_layout.setContentsMargins(5, 5, 5, 5)
        self.position_sliders = {}
        for axis, label in [('X', 'X轴位置'), ('Y', 'Y轴位置'), ('Z', 'Z轴位置')]:
            slider_layout = QVBoxLayout()
            slider_layout.setSpacing(2)
            slider_layout.setContentsMargins(0, 0, 0, 0)
            
            # 创建标题和范围标签
            title_layout = QHBoxLayout()
            title_layout.setSpacing(2)
            title_layout.setContentsMargins(0, 0, 0, 0)
            slider_label = QLabel(f"{label} ({self.position_ranges[axis][0]/1000:.1f}m - {self.position_ranges[axis][1]/1000:.1f}m)")
            title_layout.addWidget(slider_label)
            
            # 创建控制当量输入框
            step_layout = QHBoxLayout()
            step_layout.setSpacing(2)
            step_layout.setContentsMargins(0, 0, 0, 0)
            step_label = QLabel("当量(mm):")
            step_input = QTextEdit()
            step_input.setMaximumHeight(25)
            step_input.setMaximumWidth(70)
            step_input.setText(f"{self.position_step:.1f}")
            step_input.textChanged.connect(lambda t, a=axis: self.update_position_step(a, t))
            step_layout.addWidget(step_label)
            step_layout.addWidget(step_input)
            title_layout.addLayout(step_layout)
            
            slider_layout.addLayout(title_layout)
            
            # 创建滑块
            slider = QSlider(Qt.Horizontal)
            slider.setMinimum(0)
            slider.setMaximum(1000)
            slider.setValue(500)
            slider.valueChanged.connect(lambda v, a=axis: self.on_position_slider_changed(a, v))
            self.position_sliders[axis] = (slider, slider_label, step_input)
            slider_layout.addWidget(slider)
            
            position_layout.addLayout(slider_layout)
        position_group.setLayout(position_layout)
        target_layout.addWidget(position_group)

        # 创建姿态控制滑块
        attitude_group = QGroupBox("姿态控制")
        attitude_layout = QHBoxLayout()
        attitude_layout.setSpacing(2)
        attitude_layout.setContentsMargins(5, 5, 5, 5)
        self.rotation_sliders = {}
        for axis, label in [('X', 'X轴旋转'), ('Y', 'Y轴旋转'), ('Z', 'Z轴旋转')]:
            slider_layout = QVBoxLayout()
            slider_layout.setSpacing(2)
            slider_layout.setContentsMargins(0, 0, 0, 0)
            
            # 创建标题和范围标签
            title_layout = QHBoxLayout()
            title_layout.setSpacing(2)
            title_layout.setContentsMargins(0, 0, 0, 0)
            slider_label = QLabel(f"{label} ({self.rotation_range[0]}° - {self.rotation_range[1]}°)")
            title_layout.addWidget(slider_label)
            
            # 创建控制当量输入框
            step_layout = QHBoxLayout()
            step_layout.setSpacing(2)
            step_layout.setContentsMargins(0, 0, 0, 0)
            step_label = QLabel("当量(度):")
            step_input = QTextEdit()
            step_input.setMaximumHeight(25)
            step_input.setMaximumWidth(70)
            step_input.setText(f"{self.rotation_step:.1f}")
            step_input.textChanged.connect(lambda t, a=axis: self.update_rotation_step(a, t))
            step_layout.addWidget(step_label)
            step_layout.addWidget(step_input)
            title_layout.addLayout(step_layout)
            
            slider_layout.addLayout(title_layout)
            
            # 创建滑块
            slider = QSlider(Qt.Horizontal)
            slider.setMinimum(0)
            slider.setMaximum(1000)
            slider.setValue(500)
            slider.valueChanged.connect(lambda v, a=axis: self.on_rotation_slider_changed(a, v))
            self.rotation_sliders[axis] = (slider, slider_label, step_input)
            slider_layout.addWidget(slider)
            
            attitude_layout.addLayout(slider_layout)
        attitude_group.setLayout(attitude_layout)
        target_layout.addWidget(attitude_group)

        target_group.setLayout(target_layout)
        layout.addWidget(target_group)

        # 创建世界坐标系控制组
        world_frame_group = QGroupBox("世界坐标系设置")
        world_frame_layout = QGridLayout()
        
        # 创建世界坐标系旋转控制
        world_rotation_group = QGroupBox("姿态控制")
        rotation_layout = QHBoxLayout()
        rotation_layout.setSpacing(2)
        rotation_layout.setContentsMargins(0, 0, 0, 0)
        self.world_rotation_sliders = {}
        for axis, label in [('X', 'X轴旋转'), ('Y', 'Y轴旋转'), ('Z', 'Z轴旋转')]:
            slider_layout = QVBoxLayout()
            slider_layout.setSpacing(2)
            slider_layout.setContentsMargins(0, 0, 0, 0)
            
            # 创建标题和范围标签
            title_layout = QHBoxLayout()
            title_layout.setSpacing(2)
            title_layout.setContentsMargins(0, 0, 0, 0)
            slider_label = QLabel(f"{label} (-180° - 180°)")
            title_layout.addWidget(slider_label)
            
            # 创建控制当量输入框
            step_layout = QHBoxLayout()
            step_layout.setSpacing(2)
            step_layout.setContentsMargins(0, 0, 0, 0)
            step_label = QLabel("当量(度):")
            step_input = QTextEdit()
            step_input.setMaximumHeight(25)
            step_input.setMaximumWidth(70)
            step_input.setText("5.0")
            step_input.textChanged.connect(lambda t, a=axis: self.update_world_rotation_step(a, t))
            step_layout.addWidget(step_label)
            step_layout.addWidget(step_input)
            title_layout.addLayout(step_layout)
            
            slider_layout.addLayout(title_layout)
            
            # 创建滑块
            slider = QSlider(Qt.Horizontal)
            slider.setMinimum(0)
            slider.setMaximum(1000)
            slider.setValue(500)
            slider.valueChanged.connect(lambda v, a=axis: self.on_world_rotation_slider_changed(a, v))
            self.world_rotation_sliders[axis] = (slider, slider_label, step_input)
            slider_layout.addWidget(slider)
            
            rotation_layout.addLayout(slider_layout)
        
        # 创建世界坐标系平移控制
        world_translation_group = QGroupBox("位置控制")
        translation_layout = QHBoxLayout()
        translation_layout.setSpacing(2)
        translation_layout.setContentsMargins(0, 0, 0, 0)
        self.world_translation_sliders = {}
        for axis, label in [('X', 'X轴平移'), ('Y', 'Y轴平移'), ('Z', 'Z轴平移')]:
            slider_layout = QVBoxLayout()
            slider_layout.setSpacing(2)
            slider_layout.setContentsMargins(0, 0, 0, 0)
            
            # 创建标题和范围标签
            title_layout = QHBoxLayout()
            title_layout.setSpacing(2)
            title_layout.setContentsMargins(0, 0, 0, 0)
            slider_label = QLabel(f"{label} (-4000mm - 4000mm)")
            title_layout.addWidget(slider_label)
            
            # 创建控制当量输入框
            step_layout = QHBoxLayout()
            step_layout.setSpacing(2)
            step_layout.setContentsMargins(0, 0, 0, 0)
            step_label = QLabel("当量(mm):")
            step_input = QTextEdit()
            step_input.setMaximumHeight(25)
            step_input.setMaximumWidth(70)
            step_input.setText("10.0")
            step_input.textChanged.connect(lambda t, a=axis: self.update_world_translation_step(a, t))
            step_layout.addWidget(step_label)
            step_layout.addWidget(step_input)
            title_layout.addLayout(step_layout)
            
            slider_layout.addLayout(title_layout)
            
            # 创建滑块
            slider = QSlider(Qt.Horizontal)
            slider.setMinimum(0)
            slider.setMaximum(1000)
            slider.setValue(500)
            slider.valueChanged.connect(lambda v, a=axis: self.on_world_translation_slider_changed(a, v))
            self.world_translation_sliders[axis] = (slider, slider_label, step_input)
            slider_layout.addWidget(slider)
            
            translation_layout.addLayout(slider_layout)
        
        world_rotation_group.setLayout(rotation_layout)
        world_translation_group.setLayout(translation_layout)
        
        world_frame_layout.addWidget(world_rotation_group, 0, 0)
        world_frame_layout.addWidget(world_translation_group, 1, 0)
        world_frame_group.setLayout(world_frame_layout)
        layout.addWidget(world_frame_group)

        # 创建文本框用于显示变换信息
        transform_layout = QHBoxLayout()  # 横向布局

        # 创建靶标变换信息文本框
        target_transform_group = QGroupBox("靶标坐标系相对于相机坐标系的变换")
        target_transform_layout = QVBoxLayout()
        self.target_transform_text = QTextEdit()
        self.target_transform_text.setReadOnly(True)
        self.target_transform_text.setMaximumHeight(120)
        self.target_transform_text.setStyleSheet("""
            QTextEdit {
                font-family: 'Courier New';
                font-size: 12px;
                background-color: #f8f8f8;
                padding: 2px;
            }
        """)
        target_transform_layout.addWidget(self.target_transform_text)
        target_transform_group.setLayout(target_transform_layout)

        # 创建相机变换信息文本框
        camera_transform_group = QGroupBox("相机坐标系相对于世界坐标系的变换")
        camera_transform_layout = QVBoxLayout()
        self.camera_transform_text = QTextEdit()
        self.camera_transform_text.setReadOnly(True)
        self.camera_transform_text.setMaximumHeight(120)
        self.camera_transform_text.setStyleSheet("""
            QTextEdit {
                font-family: 'Courier New';
                font-size: 12px;
                background-color: #f8f8f8;
                padding: 2px;
            }
        """)
        camera_transform_layout.addWidget(self.camera_transform_text)
        camera_transform_group.setLayout(camera_transform_layout)

        # 横向添加两个GroupBox
        transform_layout.addWidget(target_transform_group)
        transform_layout.addWidget(camera_transform_group)

        # 添加按钮
        button_layout = QHBoxLayout()  # 创建水平布局来放置两个按钮
        self.show_data_button = QPushButton("显示数据表格")
        self.add_data_button = QPushButton("添加数据")
        self.show_data_button.clicked.connect(self.on_show_data_button_clicked)
        self.add_data_button.clicked.connect(self.on_add_data_button_clicked)
        button_layout.addWidget(self.show_data_button)
        button_layout.addWidget(self.add_data_button)
        transform_layout.addLayout(button_layout)
        
        # 创建数据对话框
        self.data_dialog = DataDialog(self)
        
        # 将横向布局添加到主布局
        layout.addLayout(transform_layout)

        # 初始化图像和相机内参矩阵显示
        self.update_intrinsic_matrix()
        self.update_plots()

        # 设置世界坐标系旋转滑块初值
        for axis in ['X', 'Y', 'Z']:
            min_val, max_val = -180, 180  # 你的世界旋转滑块范围
            angle = self.world_euler[axis]
            slider, _, _ = self.world_rotation_sliders[axis]
            slider.setValue(int(1000 * (angle - min_val) / (max_val - min_val)))
        # 设置世界坐标系平移滑块初值
        for axis, idx in zip(['X', 'Y', 'Z'], [0, 1, 2]):
            min_val, max_val = -4000, 4000  # 你的世界平移滑块范围
            pos = self.world_to_camera_translation[idx]
            slider, _, _ = self.world_translation_sliders[axis]
            slider.setValue(int(1000 * (pos - min_val) / (max_val - min_val)))

        # 创建数据生成控制区域
        generate_layout = QHBoxLayout()
        generate_layout.setSpacing(2)

        # 创建输入框
        self.generate_count_input = QTextEdit()
        self.generate_count_input.setMaximumHeight(25)
        self.generate_count_input.setMaximumWidth(80)
        self.generate_count_input.setPlaceholderText("生成数量")

        # 创建生成按钮
        self.generate_data_button = QPushButton("一键生成数据")
        self.generate_data_button.clicked.connect(self.generate_multiple_data)

        # 添加到布局
        generate_layout.addWidget(QLabel("生成数据组数:"))
        generate_layout.addWidget(self.generate_count_input)
        generate_layout.addWidget(self.generate_data_button)
        generate_layout.addStretch()  # 添加弹性空间

        # 将生成控制区域添加到主布局
        layout.addLayout(generate_layout)

    def update_resolution_slider(self, axis, value):
        """通过滑块更新分辨率"""
        try:
            global IMAGE_WIDTH, IMAGE_HEIGHT, PRINCIPAL_POINT_X, PRINCIPAL_POINT_Y, INTRINSIC_MATRIX
            if axis == 'X':
                IMAGE_WIDTH = value
                PRINCIPAL_POINT_X = IMAGE_WIDTH / 2
                self.resolution_x_input.setText(str(value))
            else:
                IMAGE_HEIGHT = value
                PRINCIPAL_POINT_Y = IMAGE_HEIGHT / 2
                self.resolution_y_input.setText(str(value))
            INTRINSIC_MATRIX = np.array([
                [FOCAL_LENGTH_PIXELS_X, 0, PRINCIPAL_POINT_X],
                [0, FOCAL_LENGTH_PIXELS_Y, PRINCIPAL_POINT_Y],
                [0, 0, 1]
            ])
            self.update_intrinsic_matrix()
            self.update_plots()
        except ValueError:
            pass

    def update_resolution(self, axis):
        """更新分辨率"""
        try:
            global IMAGE_WIDTH, IMAGE_HEIGHT, PRINCIPAL_POINT_X, PRINCIPAL_POINT_Y, INTRINSIC_MATRIX
            if axis == 'X':
                IMAGE_WIDTH = int(self.resolution_x_input.toPlainText())
                PRINCIPAL_POINT_X = IMAGE_WIDTH / 2
                # 更新滑块位置
                self.resolution_x_slider.setValue(IMAGE_WIDTH)
            else:
                IMAGE_HEIGHT = int(self.resolution_y_input.toPlainText())
                PRINCIPAL_POINT_Y = IMAGE_HEIGHT / 2
                # 更新滑块位置
                self.resolution_y_slider.setValue(IMAGE_HEIGHT)
            INTRINSIC_MATRIX = np.array([
                [FOCAL_LENGTH_PIXELS_X, 0, PRINCIPAL_POINT_X],
                [0, FOCAL_LENGTH_PIXELS_Y, PRINCIPAL_POINT_Y],
                [0, 0, 1]
            ])
            self.update_intrinsic_matrix()
            self.update_plots()
        except ValueError:
            pass

    def update_pixel_size(self, text=None):
        try:
            global PIXEL_SIZE, FOCAL_LENGTH_PIXELS_X, FOCAL_LENGTH_PIXELS_Y, INTRINSIC_MATRIX
            if text is not None:
                PIXEL_SIZE = float(text)  # 直接使用毫米值
                # 更新滑块位置
                self.pixel_size_slider.blockSignals(True)  # 阻止滑块信号
                self.pixel_size_slider.setValue(int(PIXEL_SIZE * 1000))
                self.pixel_size_slider.blockSignals(False)  # 恢复滑块信号
                # 更新文本框内容
                self.pixel_size_input.blockSignals(True)  # 阻止文本框信号
                self.pixel_size_input.setText(f"{PIXEL_SIZE:.4f}")
                self.pixel_size_input.blockSignals(False)  # 恢复文本框信号
            FOCAL_LENGTH_PIXELS_X = FOCAL_LENGTH_MM / PIXEL_SIZE
            FOCAL_LENGTH_PIXELS_Y = FOCAL_LENGTH_MM / PIXEL_SIZE
            INTRINSIC_MATRIX = np.array([
                [FOCAL_LENGTH_PIXELS_X, 0, PRINCIPAL_POINT_X],
                [0, FOCAL_LENGTH_PIXELS_Y, PRINCIPAL_POINT_Y],
                [0, 0, 1]
            ])
            self.update_intrinsic_matrix()
            self.update_plots()
        except ValueError:
            pass

    def update_focal_length_input(self, text=None):
        try:
            global FOCAL_LENGTH_MM, FOCAL_LENGTH_PIXELS_X, FOCAL_LENGTH_PIXELS_Y, INTRINSIC_MATRIX
            if text is not None:
                FOCAL_LENGTH_MM = float(text)
                # 更新滑块位置
                self.focal_length_slider.setValue(int(float(text)))
                # 同步更新文本框内容
                self.focal_length_input.blockSignals(True)
                self.focal_length_input.setText(f"{FOCAL_LENGTH_MM:.1f}")
                self.focal_length_input.blockSignals(False)
            FOCAL_LENGTH_PIXELS_X = FOCAL_LENGTH_MM / PIXEL_SIZE
            FOCAL_LENGTH_PIXELS_Y = FOCAL_LENGTH_MM / PIXEL_SIZE
            INTRINSIC_MATRIX = np.array([
                [FOCAL_LENGTH_PIXELS_X, 0, PRINCIPAL_POINT_X],
                [0, FOCAL_LENGTH_PIXELS_Y, PRINCIPAL_POINT_Y],
                [0, 0, 1]
            ])
            self.update_intrinsic_matrix()
            self.update_plots()
        except ValueError:
            pass

    def update_intrinsic_matrix(self):
        """更新相机内参矩阵显示"""
        matrix_text = "[\n"
        for row in INTRINSIC_MATRIX:
            matrix_text += "  ["
            matrix_text += ", ".join([f"{x:12.2f}" for x in row])
            matrix_text += "]\n"
        matrix_text += "]"
        self.intrinsic_text.setText(matrix_text)
        # 添加矩阵说明
        self.intrinsic_text.append("\n说明:")
        self.intrinsic_text.append(f"fx = {FOCAL_LENGTH_PIXELS_X:.2f} (X方向焦距)")
        self.intrinsic_text.append(f"fy = {FOCAL_LENGTH_PIXELS_Y:.2f} (Y方向焦距)")
        self.intrinsic_text.append(f"cx = {PRINCIPAL_POINT_X:.2f} (主点X坐标)")
        self.intrinsic_text.append(f"cy = {PRINCIPAL_POINT_Y:.2f} (主点Y坐标)")

    def update_position_step(self, axis, text):
        try:
            self.position_step = float(text)
        except ValueError:
            pass

    def update_rotation_step(self, axis, text):
        try:
            self.rotation_step = float(text)
        except ValueError:
            pass

    def update_position_range(self, axis, text, is_min):
        try:
            value = float(text) * 1000  # 转换为毫米
            if is_min:
                self.position_ranges[axis] = (value, self.position_ranges[axis][1])
            else:
                self.position_ranges[axis] = (self.position_ranges[axis][0], value)
            # 更新滑块标签
            slider, label, _ = self.position_sliders[axis]
            label.setText(f"{axis}轴位置 ({self.position_ranges[axis][0]/1000:.1f}m - {self.position_ranges[axis][1]/1000:.1f}m)")
            # 重置滑块位置
            current_pos = self.current_position[{'X': 0, 'Y': 1, 'Z': 2}[axis]]
            slider.setValue(int(1000 * (current_pos - self.position_ranges[axis][0]) / 
                              (self.position_ranges[axis][1] - self.position_ranges[axis][0])))
        except ValueError:
            pass

    def update_rotation_range(self, text, is_min):
        try:
            value = float(text)
            if is_min:
                self.rotation_range = (value, self.rotation_range[1])
            else:
                self.rotation_range = (self.rotation_range[0], value)
            # 更新所有旋转滑块的标签
            for axis in ['X', 'Y', 'Z']:
                slider, label, _ = self.rotation_sliders[axis]
                label.setText(f"{axis}轴旋转 ({self.rotation_range[0]}° - {self.rotation_range[1]}°)")
                # 重置滑块位置
                current_angle = np.degrees(np.arctan2(self.current_rotation[1, 0], self.current_rotation[0, 0]))
                slider.setValue(int(1000 * (current_angle - self.rotation_range[0]) / 
                                  (self.rotation_range[1] - self.rotation_range[0])))
        except ValueError:
            pass

    def on_position_slider_changed(self, axis, value):
        # 将滑块值（0-1000）映射到实际范围
        min_val, max_val = self.position_ranges[axis]
        position = min_val + (max_val - min_val) * value / 1000.0
        self.current_position[{'X': 0, 'Y': 1, 'Z': 2}[axis]] = position
        self.update_plots()

    def on_rotation_slider_changed(self, axis, value):
        # 将滑块值（0-1000）映射到实际范围
        min_val, max_val = self.rotation_range
        angle = min_val + (max_val - min_val) * value / 1000.0
        self.current_euler[axis] = angle  # 只更新当前轴的欧拉角

        # 依次生成旋转矩阵（ZYX顺序，可根据需要调整）
        rx, ry, rz = np.radians(self.current_euler['X']), np.radians(self.current_euler['Y']), np.radians(self.current_euler['Z'])
        Rx = np.array([
            [1, 0, 0],
            [0, np.cos(rx), -np.sin(rx)],
            [0, np.sin(rx), np.cos(rx)]
        ])
        Ry = np.array([
            [np.cos(ry), 0, np.sin(ry)],
            [0, 1, 0],
            [-np.sin(ry), 0, np.cos(ry)]
        ])
        Rz = np.array([
            [np.cos(rz), -np.sin(rz), 0],
            [np.sin(rz), np.cos(rz), 0],
            [0, 0, 1]
        ])
        # 组合旋转矩阵（ZYX顺序）
        self.current_rotation = Rz @ Ry @ Rx
        self.update_plots()

    def draw_camera_frame(self, ax, scale=100):
        """绘制相机坐标系，支持变换"""
        # 获取相机在世界坐标系下的位置和朝向
        R = self.world_to_camera_rotation
        T = self.world_to_camera_translation
        # 画出相机坐标系的三个轴
        origin = T
        ax.quiver(*origin, *(R[:,0]*scale), color='r', label='Xc')
        ax.quiver(*origin, *(R[:,1]*scale), color='g', label='Yc')
        ax.quiver(*origin, *(R[:,2]*scale), color='b', label='Zc')
        ax.text(*(origin + R[:,0]*scale), 'Xc', color='r')
        ax.text(*(origin + R[:,1]*scale), 'Yc', color='g')
        ax.text(*(origin + R[:,2]*scale), 'Zc', color='b')

    def draw_target_frame(self, ax, pose, scale=100):
        """绘制靶标坐标系"""
        origin = np.array([0, 0, 0])
        # 将靶标坐标系转换到相机坐标系
        target_origin = np.dot(pose, np.array([0, 0, 0, 1]))[:3]
        # X轴 - 红色
        ax.quiver(target_origin[0], target_origin[1], target_origin[2], scale, 0, 0, color='r', label='Xt')
        # Y轴 - 绿色
        ax.quiver(target_origin[0], target_origin[1], target_origin[2], 0, scale, 0, color='g', label='Yt')
        # Z轴 - 蓝色
        ax.quiver(target_origin[0], target_origin[1], target_origin[2], 0, 0, scale, color='b', label='Zt')
        ax.text(target_origin[0] + scale, target_origin[1], target_origin[2], 'Xt', color='r')
        ax.text(target_origin[0], target_origin[1] + scale, target_origin[2], 'Yt', color='g')
        ax.text(target_origin[0], target_origin[1], target_origin[2] + scale, 'Zt', color='b')

    def draw_world_frame(self, ax, scale=100):
        """始终在原点画世界坐标系"""
        origin = np.array([0, 0, 0])
        R = np.eye(3)
        ax.quiver(*origin, *(R[:,0]*scale), color='r', label='Xw')
        ax.quiver(*origin, *(R[:,1]*scale), color='g', label='Yw')
        ax.quiver(*origin, *(R[:,2]*scale), color='b', label='Zw')
        ax.text(*(origin + R[:,0]*scale), 'Xw', color='r')
        ax.text(*(origin + R[:,1]*scale), 'Yw', color='g')
        ax.text(*(origin + R[:,2]*scale), 'Zw', color='b')

    def update_world_rotation_step(self, axis, text):
        try:
            self.world_rotation_step = float(text)
        except ValueError:
            pass

    def update_world_translation_step(self, axis, text):
        try:
            self.world_translation_step = float(text)
        except ValueError:
            pass

    def on_world_rotation_slider_changed(self, axis, value):
        # 将滑块值（0-1000）映射到实际范围（-180到180度）
        angle = -180 + 360 * value / 1000.0
        self.world_euler[axis] = angle  # 只更新当前轴的欧拉角

        # 依次生成旋转矩阵（ZYX顺序，可根据需要调整）
        rx, ry, rz = np.radians(self.world_euler['X']), np.radians(self.world_euler['Y']), np.radians(self.world_euler['Z'])
        Rx = np.array([
            [1, 0, 0],
            [0, np.cos(rx), -np.sin(rx)],
            [0, np.sin(rx), np.cos(rx)]
        ])
        Ry = np.array([
            [np.cos(ry), 0, np.sin(ry)],
            [0, 1, 0],
            [-np.sin(ry), 0, np.cos(ry)]
        ])
        Rz = np.array([
            [np.cos(rz), -np.sin(rz), 0],
            [np.sin(rz), np.cos(rz), 0],
            [0, 0, 1]
        ])
        # 组合旋转矩阵（ZYX顺序）
        self.world_to_camera_rotation = Rz @ Ry @ Rx
        self.update_plots()

    def on_world_translation_slider_changed(self, axis, value):
        # 将滑块值（0-1000）映射到实际范围（-4000到4000毫米）
        translation = -4000 + 8000 * value / 1000.0
        self.world_to_camera_translation[{'X': 0, 'Y': 1, 'Z': 2}[axis]] = translation
        self.update_plots()

    def on_show_data_button_clicked(self):
        try:
            if not hasattr(self, 'last_projected_points'):
                print("Error: last_projected_points not found")
                return
            
            pixel_coords = self.last_projected_points
            R = self.current_rotation
            T = self.current_position
            R_wc = self.world_to_camera_rotation
            T_wc = self.world_to_camera_translation
            P_w = R_wc @ T + T_wc
            
            print(f"Data prepared: {len(pixel_coords)} points")
            
            # 更新对话框数据，传入相机外参
            self.data_dialog.update_data(
                pixel_coords, R, T, P_w, 
                world_rotation=R_wc,
                world_translation=T_wc,
                clear_table=True
            )
            
            self.data_dialog.raise_()
            self.data_dialog.activateWindow()
            self.data_dialog.show()
            
        except Exception as e:
            print(f"Error in on_show_data_button_clicked: {str(e)}")

    def on_add_data_button_clicked(self):
        try:
            if not hasattr(self, 'last_projected_points'):
                print("Error: last_projected_points not found")
                return
            
            pixel_coords = self.last_projected_points
            R = self.current_rotation
            T = self.current_position
            R_wc = self.world_to_camera_rotation
            T_wc = self.world_to_camera_translation
            P_w = R_wc @ T + T_wc
            
            print(f"Adding new data: {len(pixel_coords)} points")
            
            # 更新对话框数据，传入相机外参
            self.data_dialog.update_data(
                pixel_coords, R, T, P_w,
                world_rotation=R_wc,
                world_translation=T_wc,
                clear_table=False
            )
            
            self.data_dialog.raise_()
            self.data_dialog.activateWindow()
            self.data_dialog.show()
            
        except Exception as e:
            print(f"Error in on_add_data_button_clicked: {str(e)}")

    def update_plots(self):
        # 构建世界坐标系到相机坐标系的变换矩阵
        world_to_camera = np.eye(4)
        world_to_camera[:3, :3] = self.world_to_camera_rotation
        world_to_camera[:3, 3] = self.world_to_camera_translation

        # 构建靶标到相机坐标系的变换矩阵
        target_to_camera = np.eye(4)
        target_to_camera[:3, :3] = self.current_rotation
        target_to_camera[:3, 3] = self.current_position

        # 将靶标点转换到相机坐标系
        target_points_camera = np.dot(target_to_camera, np.vstack((target_points.T, np.ones((1, 6)))))[:3].T

        # 修改投影计算部分
        # 不需要再次进行坐标变换，因为点已经在相机坐标系下
        projected_points = np.zeros((len(target_points_camera), 2))
        for i, point in enumerate(target_points_camera):
            if point[2] > 0:  # 确保点在相机前方
                # 透视投影
                x = point[0] / point[2]
                y = point[1] / point[2]
                # 应用相机内参
                u = FOCAL_LENGTH_PIXELS_X * x + PRINCIPAL_POINT_X
                v = FOCAL_LENGTH_PIXELS_Y * y + PRINCIPAL_POINT_Y
                projected_points[i] = [u, v]
            else:
                # 如果点在相机后方，设置为图像边界外的位置
                projected_points[i] = [-100, -100]

        # 清除旧图
        self.ax_3d.clear()
        self.ax_2d.clear()

        # 绘制世界坐标系
        self.draw_world_frame(self.ax_3d)
        
        # 绘制相机坐标系
        self.draw_camera_frame(self.ax_3d)
        
        # 绘制靶标坐标系
        self.draw_target_frame(self.ax_3d, target_to_camera)
        
        # 绘制3D点
        scatter = self.ax_3d.scatter(target_points_camera[:, 0], 
                                   target_points_camera[:, 1], 
                                   target_points_camera[:, 2], 
                                   c='r', marker='o')
        
        # 添加3D点编号
        for i, point in enumerate(target_points_camera):
            self.ax_3d.text(point[0], point[1], point[2], f'P{i+1}', 
                          color='blue', fontsize=10, fontweight='bold')
        
        # 绘制所有点对之间的虚线
        for i in range(len(target_points_camera)):
            for j in range(i+1, len(target_points_camera)):
                self.ax_3d.plot([target_points_camera[i, 0], target_points_camera[j, 0]],
                              [target_points_camera[i, 1], target_points_camera[j, 1]],
                              [target_points_camera[i, 2], target_points_camera[j, 2]], 
                              'k--', alpha=0.3)

        # 设置3D图属性
        self.ax_3d.set_xlabel('X (mm)')
        self.ax_3d.set_ylabel('Y (mm)')
        self.ax_3d.set_zlabel('Z (mm)')
        self.ax_3d.set_title('3D Space Position')
        self.ax_3d.legend()

        # 修改2D投影点的绘制
        projected_points = np.array(projected_points)
        valid_points = projected_points[:, 0] >= 0
        self.ax_2d.scatter(projected_points[valid_points, 0], 
                          projected_points[valid_points, 1],
                          c='blue', s=50, marker='o')
        
        # 添加2D点编号和坐标（只显示有效点）
        for i, point in enumerate(projected_points):
            if point[0] >= 0:  # 只显示有效点的标签
            # 添加点编号
            self.ax_2d.text(point[0], point[1], f'P{i+1}', 
                          color='blue', fontsize=10, fontweight='bold',
                              ha='right', va='bottom')
            # 添加坐标值
            self.ax_2d.text(point[0], point[1] + 15, 
                              f'({point[0]:.1f}, {point[1]:.1f})',
                          color='black', fontsize=8,
                              ha='right', va='bottom')
        
        # 设置2D图属性
        self.ax_2d.set_xlabel('X (pixels)')
        self.ax_2d.set_ylabel('Y (pixels)')
        self.ax_2d.set_title('2D Projection')
        self.ax_2d.set_xlim(0, IMAGE_WIDTH)
        self.ax_2d.set_ylim(IMAGE_HEIGHT, 0)  # 注意：图像坐标系y轴向下
        self.ax_2d.grid(True)
        
        # 设置坐标轴位置
        self.ax_2d.spines['left'].set_position('zero')
        self.ax_2d.spines['bottom'].set_position('zero')
        self.ax_2d.spines['right'].set_visible(False)
        self.ax_2d.spines['top'].set_visible(False)
        
        # 移动X轴到顶部
        self.ax_2d.xaxis.set_ticks_position('top')
        self.ax_2d.xaxis.set_label_position('top')
        
        # 移动Y轴到左侧
        self.ax_2d.yaxis.set_ticks_position('left')
        self.ax_2d.yaxis.set_label_position('left')
        
        # 添加坐标轴说明
        self.ax_2d.text(IMAGE_WIDTH-100, -20, 'X', color='red', fontsize=12, fontweight='bold')
        self.ax_2d.text(-20, IMAGE_HEIGHT-20, 'Y', color='green', fontsize=12, fontweight='bold')

        # 计算世界坐标系到相机坐标系的欧拉角
        world_rotation_euler = np.degrees(np.array([
            np.arctan2(self.world_to_camera_rotation[2, 1], self.world_to_camera_rotation[2, 2]),
            np.arctan2(-self.world_to_camera_rotation[2, 0], np.sqrt(self.world_to_camera_rotation[2, 1]**2 + self.world_to_camera_rotation[2, 2]**2)),
            np.arctan2(self.world_to_camera_rotation[1, 0], self.world_to_camera_rotation[0, 0])
        ]))
        
        # 计算靶标到相机坐标系的欧拉角
        target_rotation_euler = np.degrees(np.array([
            np.arctan2(self.current_rotation[2, 1], self.current_rotation[2, 2]),
            np.arctan2(-self.current_rotation[2, 0], np.sqrt(self.current_rotation[2, 1]**2 + self.current_rotation[2, 2]**2)),
            np.arctan2(self.current_rotation[1, 0], self.current_rotation[0, 0])
        ]))
        
        # 格式化世界坐标系到相机坐标系的旋转矩阵
        world_rotation_matrix_str = "旋转矩阵 R:\n"
        for row in self.world_to_camera_rotation:
            world_rotation_matrix_str += "["
            world_rotation_matrix_str += ", ".join([f"{x:8.3f}" for x in row])
            world_rotation_matrix_str += "]\n"
            
        # 格式化靶标到相机坐标系的旋转矩阵
        target_rotation_matrix_str = "旋转矩阵 R:\n"
        for row in self.current_rotation:
            target_rotation_matrix_str += "["
            target_rotation_matrix_str += ", ".join([f"{x:8.3f}" for x in row])
            target_rotation_matrix_str += "]\n"
        
        # 更新靶标变换信息显示
        self.target_transform_text.setText(
            f"欧拉角: [{target_rotation_euler[0]:.2f}, {target_rotation_euler[1]:.2f}, {target_rotation_euler[2]:.2f}]°\n"
            f"平移向量 T: [{self.current_position[0]:.2f}, {self.current_position[1]:.2f}, {self.current_position[2]:.2f}] mm\n"
            f"旋转矩阵 R:\n" +
            "\n".join(
                "[" + ", ".join(f"{x:8.3f}" for x in row) + "]"
                for row in self.current_rotation
            )
        )
        
        # 更新相机变换信息显示
        self.camera_transform_text.setText(
            f"欧拉角: [{world_rotation_euler[0]:.2f}, {world_rotation_euler[1]:.2f}, {world_rotation_euler[2]:.2f}]°\n"
            f"平移向量 T: [{self.world_to_camera_translation[0]:.2f}, {self.world_to_camera_translation[1]:.2f}, {self.world_to_camera_translation[2]:.2f}] mm\n"
            f"旋转矩阵 R:\n" +
            "\n".join(
                "[" + ", ".join(f"{x:8.3f}" for x in row) + "]"
                for row in self.world_to_camera_rotation
            )
        )

        # 保存投影点坐标供其他函数使用
        self.last_projected_points = projected_points.reshape(-1, 2)
        print(f"Updated last_projected_points: {self.last_projected_points.shape}")  # 调试信息

        # 更新画布
        self.fig.subplots_adjust(left=0.1, right=0.9, top=0.9, bottom=0.1)
        self.canvas.draw()

    def generate_multiple_data(self):
        """一键生成多组数据"""
        try:
            # 获取要生成的数据组数
            count_text = self.generate_count_input.toPlainText()
            if not count_text:
                print("请输入要生成的数据组数")
                return
            
            count = int(count_text)
            if count <= 0:
                print("生成数据组数必须大于0")
                return

            # 根据相机内参计算视场角
            fov_h = 2 * np.arctan(IMAGE_WIDTH / (2 * FOCAL_LENGTH_PIXELS_X))  # 水平视场角
            fov_v = 2 * np.arctan(IMAGE_HEIGHT / (2 * FOCAL_LENGTH_PIXELS_Y))  # 垂直视场角
            
            # 计算靶标尺寸
            target_size = np.max(np.abs(target_points))  # 获取靶标最大尺寸
            
            # 根据视场角和靶标尺寸计算最小工作距离
            min_distance = target_size * 1.2 / np.tan(min(fov_h, fov_v) / 2)
            max_distance = min_distance * 2  # 最大距离设为最小距离的2倍
            
            # 禁用生成按钮，避免重复点击
            self.generate_data_button.setEnabled(False)
            
            # 创建定时器
            timer = QTimer()
            current_count = [0]  # 使用列表存储当前计数，以便在lambda中修改
            
            def generate_single_data():
                if current_count[0] >= count:
                    # 生成完成，停止定时器并重新启用按钮
                    timer.stop()
                    self.generate_data_button.setEnabled(True)
                    return

                valid_data = False
                max_attempts = 30  # 最大尝试次数
                attempts = 0

                while not valid_data and attempts < max_attempts:
                    attempts += 1
                    
                    try:
                        # 1. 生成基础位置（在视场内）
                        base_distance = np.random.uniform(min_distance, max_distance)
                        
                        # 计算当前距离下的可视范围
                        max_x = base_distance * np.tan(fov_h * 0.4)  # 使用80%的视场范围
                        max_y = base_distance * np.tan(fov_v * 0.4)
                        
                        # 在可视范围内生成位置
                        x = np.random.uniform(-max_x, max_x)
                        y = np.random.uniform(-max_y, max_y)
                        z = base_distance
                        
                        random_position = np.array([x, y, z])
                        
                        # 2. 生成基础姿态（使靶标朝向相机）
                        # 计算从靶标指向相机的向量
                        to_camera = -random_position / np.linalg.norm(random_position)
                        
                        # 构建旋转矩阵的基础
                        z_axis = to_camera
                        temp_up = np.array([0, 1, 0])  # 临时上向量
                        
                        # 如果视线方向接近垂直，选择不同的临时上向量
                        if abs(np.dot(z_axis, temp_up)) > 0.9:
                            temp_up = np.array([1, 0, 0])
                            
                        x_axis = np.cross(temp_up, z_axis)
                        x_axis = x_axis / np.linalg.norm(x_axis)
                        y_axis = np.cross(z_axis, x_axis)
                        y_axis = y_axis / np.linalg.norm(y_axis)
                        
                        # 构建基础旋转矩阵
                        R_base = np.column_stack((x_axis, y_axis, z_axis))
                        
                        # 3. 添加小幅随机旋转（确保靶标不会偏离太多）
                        angle_range = 20  # 最大偏转角度
                        rx = np.radians(np.random.uniform(-angle_range, angle_range))
                        ry = np.radians(np.random.uniform(-angle_range, angle_range))
                        rz = np.radians(np.random.uniform(-angle_range, angle_range))
                        
                        Rx = np.array([
                            [1, 0, 0],
                            [0, np.cos(rx), -np.sin(rx)],
                            [0, np.sin(rx), np.cos(rx)]
                        ])
                        
                        Ry = np.array([
                            [np.cos(ry), 0, np.sin(ry)],
                            [0, 1, 0],
                            [-np.sin(ry), 0, np.cos(ry)]
                        ])
                        
                        Rz = np.array([
                            [np.cos(rz), -np.sin(rz), 0],
                            [np.sin(rz), np.cos(rz), 0],
                            [0, 0, 1]
                        ])
                        
                        # 组合旋转矩阵
                        R_random = Rz @ Ry @ Rx
                        self.current_rotation = R_random @ R_base
                        
                        # 4. 更新位置和欧拉角
                        self.current_position = random_position
                        self.current_euler = {
                            'X': np.degrees(rx),
                            'Y': np.degrees(ry),
                            'Z': np.degrees(rz)
                        }
                        
                        # 5. 验证所有点是否在视野内
                        target_to_camera = np.eye(4)
                        target_to_camera[:3, :3] = self.current_rotation
                        target_to_camera[:3, 3] = self.current_position
                        
                        # 将靶标点转换到相机坐标系
                        target_points_camera = np.dot(target_to_camera, np.vstack((target_points.T, np.ones((1, 6)))))[:3].T
                        
                        # 检查所有点是否在相机前方且投影在图像范围内
                        all_points_visible = True
                        margin = 50  # 边距（像素）
                        min_z = 100  # 最小Z距离（毫米）
                        
                        for point in target_points_camera:
                            if point[2] <= min_z:  # 检查Z距离
                                all_points_visible = False
                                break
                                
                            # 计算投影点
                            x = point[0] / point[2]
                            y = point[1] / point[2]
                            u = FOCAL_LENGTH_PIXELS_X * x + PRINCIPAL_POINT_X
                            v = FOCAL_LENGTH_PIXELS_Y * y + PRINCIPAL_POINT_Y
                            
                            # 检查是否在图像范围内
                            if not (margin <= u <= IMAGE_WIDTH-margin and margin <= v <= IMAGE_HEIGHT-margin):
                                all_points_visible = False
                                break
                        
                        if all_points_visible:
                            valid_data = True
                            
                            # 更新位置滑块
                            for axis, (slider, _, _) in self.position_sliders.items():
                                min_val, max_val = self.position_ranges[axis]
                                pos = self.current_position[{'X': 0, 'Y': 1, 'Z': 2}[axis]]
                                slider_value = int(1000 * (pos - min_val) / (max_val - min_val))
                                slider.setValue(slider_value)
                            
                            # 更新姿态滑块
                            for axis, (slider, _, _) in self.rotation_sliders.items():
                                min_val, max_val = self.rotation_range
                                angle = self.current_euler[axis]
                                slider_value = int(1000 * (angle - min_val) / (max_val - min_val))
                                slider.setValue(slider_value)
                            
                            # 更新显示
                            self.update_plots()
                            
                            # 添加数据到对话框
                            if hasattr(self, 'last_projected_points'):
                                pixel_coords = self.last_projected_points
                                R = self.current_rotation
                                T = self.current_position
                                R_wc = self.world_to_camera_rotation
                                T_wc = self.world_to_camera_translation
                                P_w = R_wc @ T + T_wc
                                
                                # 更新对话框数据
                                self.data_dialog.update_data(
                                    pixel_coords, R, T, P_w,
                                    world_rotation=R_wc,
                                    world_translation=T_wc,
                                    clear_table=False
                                )
                    except Exception as e:
                        print(f"生成数据时出错: {str(e)}")
                        continue
                
                if not valid_data:
                    print(f"第 {current_count[0]+1} 组数据生成失败，无法找到合适的位置和姿态")
                    return
                
                # 显示进度
                print(f"已生成第 {current_count[0]+1}/{count} 组数据")
                current_count[0] += 1
                
                # 如果是最后一组数据，显示对话框
                if current_count[0] >= count:
                    self.data_dialog.raise_()
                    self.data_dialog.activateWindow()
                    self.data_dialog.show()
            
            # 设置定时器间隔（500毫秒）并启动
            timer.timeout.connect(generate_single_data)
            timer.start(500)  # 每500毫秒生成一组数据
            
        except ValueError as e:
            print(f"输入错误: {str(e)}")
            self.generate_data_button.setEnabled(True)
        except Exception as e:
            print(f"生成数据时出错: {str(e)}")
            self.generate_data_button.setEnabled(True)

    def on_pixel_size_slider_changed(self, value):
        """处理像元尺寸滑块值改变的事件"""
        pixel_size_mm = value / 1000.0  # 将滑块值转换为毫米
        self.update_pixel_size(str(pixel_size_mm))

if __name__ == "__main__":
    app = QApplication(sys.argv)
    window = MainWindow()
    window.show()
    sys.exit(app.exec())

